#include <chassis_controller/pid.h>

PID::PID(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node):node_(node)
{
    clearCtrl();
}

double PID::incrementalPID(double input, double setpoint, bool halt, bool reset)
{
    return output_;
}

double PID::positionPID(double input, double setpoint, bool halt, bool reset)
{   
    double d_err = err_ - err1_;
    err_ = setpoint - input;
    if (abs(err_) < pid_param_.integral_threshold && abs(err_sum_) < pid_param_.integral_limit && !halt){
        err_sum_ += err_ * pid_param_.period;
    }
    else {
        err_sum_ = 0.0;
    }
    
    p_term_ = pid_param_.Kp * err_;
    i_term_ = pid_param_.Ki * err_sum_;
    d_term_ = pid_param_.Kd * d_err;
    RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
        200, "P  I  D term: "<<p_term_ <<","<<i_term_<<","<<d_term_);
    //halt
    if (halt)
        output_ = output_last_;
    else
        output_ = p_term_ + i_term_ + d_term_;

    //limit
    if(output_ > pid_param_.upper_limit)
        output_ = pid_param_.upper_limit;
    else if (output_ < pid_param_.lower_limit)
        output_ = pid_param_.lower_limit;

    //invert
    if (pid_param_.inverted)
        output_ *= -1.0;

    //reset
    if (reset)
        clearCtrl();    
    
    //save last value
    output_last_ = output_;
    err2_ = err1_;
    err1_ = err_;

    return output_;
}

void PID::clearCtrl()
{
	err_ = 0.0;
	err1_ = 0.0;
	err2_ = 0.0;
	i_term_ = 0.0;
    p_term_ = 0.0;
    d_term_ = 0.0;
    output_ = 0.0;
    output_last_ = 0.0;
    err_sum_ = 0.0;
}
